﻿using HelixToolkit.Wpf;
using MathNet.Numerics.LinearAlgebra.Double;
using SharpDX;
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Numerics;
using System.Security.Policy;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Documents;
using System.Windows.Media.Media3D;
using Quaternion = System.Windows.Media.Media3D.Quaternion;

namespace TeaStlShow
{
    public  class TeathCuttingData
    {
        /// <summary>
        /// 是否找到stl文件
        /// </summary>
        bool FindStl = false;
        /// <summary>
        /// 是否找到PTS文件
        /// </summary>
        bool FindPts = false;
        /// <summary>
        /// 路径优化
        /// </summary>
        public CurveSmoother SmootherPath;
        /// <summary>
        /// STL的问价夹位置
        /// </summary>
        public static string StlDirectory;
        /// <summary>
        /// 牙龈先的二维码
        /// </summary>
        public string Code;
        /// <summary>
        /// 牙齿的模型
        /// </summary>
        public Model3DGroup stlModelYuan;
        /// <summary>
        /// 牙龈先的点云原始
        /// </summary>
        public List<Point3D> pointCloudYuan;
        /// <summary>
        /// 点的法线原始
        /// </summary>
        public List<Vector3D> normalLinesyuanfa;

        /// <summary>
        /// 点位需要排序
        /// </summary>
        public List<Point3D> SortpointCloudYuan;
        /// <summary>
        /// 法线跟着点位排序
        /// </summary>
        public List<Vector3D> SortnormalLinesyuanfa;

        /// <summary>
        /// 插点后的点
        /// </summary>
        public List<Point3D> pointCloudPai;
        /// <summary>
        /// 插点后点的法线
        /// </summary>
        public List<Vector3D> normalLinespaifa;
        /// <summary>
        /// 点的法线
        /// </summary>
        public List<Vector3D> normalLinesFZ;
        /// <summary>
        /// 点的x方向
        /// </summary>
        public List<Vector3D> normalLinesFX;
        /// <summary>
        /// 点的y的防线
        /// </summary>
        public List<Vector3D> normalLinesFY;

        /// <summary>
        /// 初始化文件夹位置
        /// </summary>
        /// <param name="directory"></param>
        public void Init()
        {
            if (SmootherPath == null)
            {
                SmootherPath = new CurveSmoother();
            }
            if (pointCloudYuan == null)
            {
                pointCloudYuan = new List<Point3D>();
            }
            if (normalLinesyuanfa == null)
            {
                normalLinesyuanfa = new List<Vector3D>();
            }
            if (pointCloudPai == null)
            {
                pointCloudPai = new List<Point3D>();
            }
            if (normalLinespaifa == null)
            {
                normalLinespaifa = new List<Vector3D>();
            }
            if (normalLinesFZ == null)
            {
                normalLinesFZ = new List<Vector3D>();
            }
            if (normalLinesFX == null)
            {
                normalLinesFX = new List<Vector3D>();
            }
            if (normalLinesFY == null)
            {
                normalLinesFY = new List<Vector3D>();
            }
            if (SortpointCloudYuan == null)
            {
                SortpointCloudYuan=new List<Point3D>();
            }
            if (SortnormalLinesyuanfa == null)
            {
                SortnormalLinesyuanfa=new List<Vector3D>();
            }
        }
        /// <summary>
        /// 设置二维码
        /// </summary>
        /// <returns></returns>
        public bool SetCode(string code)
        {
            Code = code;
            if (StlDirectory != null && Code != null)
            {
                if (Directory.Exists(StlDirectory))//文件夹存在
                {
                    string stlpath = $"{StlDirectory}\\{code}.stl";
                    string ptsfile = $"{StlDirectory}\\{code}.pts";
                    if (File.Exists(stlpath))
                    {
                        var reader = new StLReader();
                        stlModelYuan = reader.Read(stlpath);
                        if (stlModelYuan.CanFreeze)
                        {
                            stlModelYuan.Freeze();
                        }
                        FindStl = true;
                    }
                    else
                    {
                        FindStl = false;
                    }
                    if (File.Exists(ptsfile))
                    {
                        var lines = File.ReadAllLines(ptsfile);
                        foreach (var line in lines)
                        {
                            if (string.IsNullOrWhiteSpace(line)) continue;
                            var parts = line.Split(new[] { ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
                            if (parts.Length >= 3)
                            {
                                var point = new Point3D(double.Parse(parts[0]), double.Parse(parts[1]), double.Parse(parts[2]));
                                pointCloudYuan.Add(point);
                                // 如果有法向信息（通常在第4-6列）
                                if (parts.Length >= 6)
                                {
                                    var normal = new Vector3D(double.Parse(parts[3]), double.Parse(parts[4]), double.Parse(parts[5]));
                                    var normal1 = new Vector3D(double.Parse(parts[3]), double.Parse(parts[4]), double.Parse(parts[5]));
                                    normalLinesyuanfa.Add(normal);
                                    normal1.Normalize();
                                    normalLinesFZ.Add(-normal1);
                                }
                                else
                                {
                                    MessageBox.Show($"PTS没有法相信息{Code}");
                                    return false;
                                }
                            }
                        }
                        FindPts = true;
                    }
                    else
                    {
                        FindPts = false;
                    }
                }
                if (FindStl && FindPts)
                {
                    if (Calculate())
                    {
                        return true;
                    }
                }
                else
                {
                    System.Windows.MessageBox.Show($"{Code}未找到stl或者pts");
                }
            }
            return false;
        }

        /// <summary>
        /// 计算路径和发现
        /// </summary>
        /// <returns></returns>
        public bool Calculate()
        {
            ShortPointCloud();
            for (int i = 0; i < normalLinesFZ.Count; i++)
            {
                Vector3D fx;
                fx = Vector3D.CrossProduct(new Vector3D(0, 0, 1), normalLinesFZ[i]); ;
                fx.Normalize();
                fx = ProjectVectorToPlane(fx, normalLinesFZ[i]);
                Vector3D fy = Vector3D.CrossProduct(fx, normalLinesFZ[i]);
                fy.Normalize();
                if (fy.Z < 0)
                {
                    fy = -fy;
                }
                normalLinesFY.Add(fy);
                fx = Vector3D.CrossProduct(normalLinesFY[i], normalLinesFZ[i]);
                fx.Normalize();
                normalLinesFX.Add(fx);

                //让X水平
                Vector3D fxs = normalLinesFX[i];
            }
            return true;
        }

        /// <summary>
        /// 求一个向量在另一个向量垂直平面上的投影
        /// </summary>
        /// <param name="X"></param>
        /// <param name="n"></param>
        /// <returns></returns>
        public Vector3D ProjectVectorToPlane(Vector3D X, Vector3D n)
        {
            Vector3D nUnit = n;
            nUnit.Normalize();

            double dot = Vector3D.DotProduct(X, nUnit);
            Vector3D projection = X - dot * nUnit;
            projection.Normalize();
            return projection;
        }

        /// <summary>
        /// 找点云的拟合圆
        /// </summary>
        /// <param name="points"></param>
        /// <returns></returns>
        public  (double a, double b, double r) FitCircle(List<Point3D> points)
        {
            int n = points.Count;
            var A = DenseMatrix.Create(n, 3, 0);
            var b = DenseVector.Create(n, 0);

            for (int i = 0; i < n; i++)
            {
                double x = points[i].X;
                double y = points[i].Y;
                A[i, 0] = x;
                A[i, 1] = y;
                A[i, 2] = 1;
                b[i] = -(x * x + y * y);
            }

            // 最小二乘解
            var p = (A.TransposeThisAndMultiply(A)).Inverse() * A.TransposeThisAndMultiply(b);

            double D = p[0];
            double E = p[1];
            double F = p[2];

            double a = -D / 2;
            double b_ = -E / 2;
            double r = Math.Sqrt(a * a + b_ * b_ - F);

            return (a, b_, r);
        }

        /// <summary>
        /// 点位的排序,按照X值的最大值进行的排序
        /// </summary>
        public void ShortPointCloud()
        {
            double maxX=0;
            int maxIndex = 0;
            for (int i = 0; i < pointCloudYuan.Count; i++)
            {
                    if (pointCloudYuan[i].X > maxX)
                    {
                        maxX = pointCloudYuan[i].X;
                        maxIndex = i;
                    }
            }//找到X的最大值点
            SortpointCloudYuan.Clear();
            SortnormalLinesyuanfa.Clear();
            if (maxIndex  < pointCloudYuan.Count)
            {
                List<Vector3D> sortnormalLinesFY = new List<Vector3D>();
                if (pointCloudYuan[maxIndex + 1>= pointCloudYuan.Count?0: maxIndex + 1].Y > pointCloudYuan[maxIndex].Y)//逆时针旋转 从上看
                {
                    for (int i = maxIndex; i < pointCloudYuan.Count; i++)
                    {
                        SortpointCloudYuan.Add(pointCloudYuan[i]);
                        sortnormalLinesFY.Add(normalLinesFZ[i]);
                        SortnormalLinesyuanfa.Add(normalLinesyuanfa[i]);
                    }
                    for (int i = 0; i < maxIndex; i++)
                    {
                        SortpointCloudYuan.Add(pointCloudYuan[i]);
                        sortnormalLinesFY.Add(normalLinesFZ[i]);
                        SortnormalLinesyuanfa.Add(normalLinesyuanfa[i]);
                    }
                    normalLinesFZ = sortnormalLinesFY;
                }
                else//顺时针旋转 从上看
                {
                    for (int i = maxIndex; i >=0; i--)
                    {
                        SortpointCloudYuan.Add(pointCloudYuan[i]);
                        sortnormalLinesFY.Add(normalLinesFZ[i]);
                        SortnormalLinesyuanfa.Add(normalLinesyuanfa[i]);
                    }
                    for (int i = pointCloudYuan.Count-1; i > maxIndex; i--)
                    {
                        SortpointCloudYuan.Add(pointCloudYuan[i]);
                        sortnormalLinesFY.Add(normalLinesFZ[i]);
                        SortnormalLinesyuanfa.Add(normalLinesyuanfa[i]);
                    }
                    normalLinesFZ = sortnormalLinesFY;
                }
            }
        }

        public bool ExcuteToFile(string path)
        {
            if (Directory.Exists(path))
            {
                string FName = "DentalCutting";
                StringBuilder moddata = new StringBuilder();
                StringBuilder modefun = new StringBuilder();
                moddata.Append($"MODULE {FName}sharp\r\n");
                modefun.Append($"PROC {FName}sharpa()\r\nSingArea\\Off;\r\nConfL\\Off;\r\n");
                modefun.Append("AccSet 20,10;\r\n");
                for (int i = 0; i < SortpointCloudYuan.Count; i++)
                {
                    Quaternion quaternion = ExcuteCuttingPathToAbb.VectorsToQuaternion(normalLinesFX[i], normalLinesFY[i], normalLinesFZ[i]);
                    var anseuar = ExcuteCuttingPathToAbb.QuaternionToEulerRadians(quaternion);
                    var yaw = anseuar.yaw * 180 / Math.PI; //Z
                    var pitch = anseuar.pitch * 180 / Math.PI;//Y
                    var roll = anseuar.roll * 180 / Math.PI;//X
                    if (i == 0)//第一个点移动到进到点
                    {
                        Point3D point = SortpointCloudYuan[i] - normalLinesFZ[i] * 20;//抬高单位数
                        moddata.Append($"LOCAL CONST robtarget LPOS0{i}:=[[{point.X}, {point.Y}, {point.Z}],[{quaternion.W},{quaternion.X},{quaternion.Y},{quaternion.Z}],[{0},{0},{0},0],[9E9,9E9,9E9,9E9,9E9,9E9]];\r\n");
                        modefun.Append($"MoveL LPOS0{i}, v200, z10, ToolTcp{1}\\WObj:= Wobj{2};\r\n");
                    }
                    //LOCAL CONST robtarget Target20:=[[-32.37, -11.58, 8.83],[0.35307263914485,0.58911269885624,0.63844430447081,0.34738279983671],[-1,0,-1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
                    moddata.Append($"LOCAL CONST robtarget LPOS{i}:=[[{SortpointCloudYuan[i].X}, {SortpointCloudYuan[i].Y}, {SortpointCloudYuan[i].Z}],[{quaternion.W},{quaternion.X},{quaternion.Y},{quaternion.Z}],[{0},{0},{0},0],[9E9,9E9,9E9,9E9,9E9,9E9]];\r\n");
                    //MoveL Target230, v200, z10, currentTool\WObj:= currentWobj;
                    modefun.Append($"MoveL LPOS{i}, v200, z10, ToolTcp{1}\\WObj:= Wobj{2};!Z:{yaw},Y:{pitch},X:{roll}\r\n");
                }
                modefun.Append($"ConfL\\Off;\r\n");
                modefun.Append($"ENDPROC\r\n");
                modefun.Append($"ENDMODULE\r\n");
                moddata.Append(modefun.ToString());
                File.WriteAllText($"{path}\\{Code}.mod", moddata.ToString());
            }
            else
            {
                return false;
            }
            return true;
        }
    }
}
